#ifndef EQUATION_H
#define EQUATION_H

#include "matrix.h"

//高斯牛顿法
template <typename T, int M,int N> //M个方程，N个未知数
class C_GN_NLS //高斯牛顿法GaussNewton 非线性最小二乘
{
public:
	int MAX_ITER; //最大迭代次数
	CMatrix<T,N,1> xtemp; //delta x
	CMatrix<T,N,1> xin;//初始解/最终解
	CMatrix<T,M,1> fout; //误差
	CMatrix<T,M,N> J; //雅克比矩阵
	CMatrix<T,N,M> JT; //雅克比矩阵的转置
	CMatrix<T,N,N> Jtemp; //JT*J
	CMatrix<T,N,N> Jtemp1; //(JT*J)`
	CMatrix<T,N,M> JTtemp; //(JT*J)`JT
//方程组
	virtual void func(void)=0; //输入输出都是列向量
//方程组的雅克比矩阵
	virtual void jacobi(void)=0;
	int run(void) //缓存
	{
		int i,j,k;
		float mse=0;
		float pre_mse=0;
		for(i=0; i < MAX_ITER; i++) //迭代次数
		{
			//首先求此解条件下的误差
			func();
			mse=fout.sq_sum(); //此解的误差
			if((fabs(pre_mse-mse)) < 1e-6)
			{
				break; //退出条件
			}
			pre_mse=mse;
			jacobi(); //求雅克比矩阵
			J.t(JT); //求雅克比矩阵的转置
			JT.mul(J,Jtemp); //JT*J
			//阻尼最小二乘
			for(int j=0;j<N;j++)
			{
				Jtemp.pd[j][j]+=0.2;
			}
			//
			Jtemp.inv(Jtemp1);
			Jtemp1.mul(JT,JTtemp);
			JTtemp.mul(fout,xtemp); //得到xtemp就是delta x
			xin.minus(xtemp);
			
			printf("%d: %f\r\n", i, mse);
		//printf("%.2f	%.2f\r\n",xin.pd[0][0],xin.pd[1][0]);
	}
	if(i==MAX_ITER)
	{
		return 1;
	}
	return 0;
}
C_GN_NLS()
{
	MAX_ITER=50;
}
~C_GN_NLS(){}
};
class C_GN_Sample : public C_GN_NLS<float,2,2>
{
public:
//方程组
virtual void func(void) //输入输出都是列向量
{
	fout.pd[0][0]=xin.pd[0][0]*xin.pd[0][0]+xin.pd[1][0]*xin.pd[1][0]-1;
	fout.pd[1][0]=xin.pd[0][0]+xin.pd[1][0]-1;
}
//方程组的雅克比矩阵
virtual void jacobi(void)
{
	J.pd[0][0]=2*xin.pd[0][0];
	J.pd[0][1]=2*xin.pd[1][0];
	J.pd[1][0]=1;
	J.pd[1][1]=1;
}
};

#endif

